package fri.pipt.agent.deathrow.path;

import java.util.ArrayList;
import java.util.LinkedList;
import java.util.PriorityQueue;
import java.util.Stack;

import javax.sound.midi.SysexMessage;

import fri.pipt.agent.deathrow.Pos2;
import fri.pipt.agent.deathrow.World;

public class PathPlanner
{
	Pos2 from, to;
	PathNode currentNode = null;
	Stack<PathNode> pathToFollow;
	
	public PathPlanner()
	{
		from = new Pos2();
		to = new Pos2();
		pathToFollow = new Stack<PathNode>();
	}
	
	/**
	 * Izračuna pot, vse nastavi na začetek
	 */
	public void planPath(World world, Pos2 from, Pos2 to)
	{
		this.from.set(from);
		this.to.set(to);
		System.out.println("Plannin path from: " + from + " to " + to);
		World agentWorld = world;
		Pos2 positionFrom = from; 
		//Po dijkstri iščemo pot
		
		int w = agentWorld.getWidth();
		int h = agentWorld.getHeight();
		if(w == 0 || h == 0)
		{
			System.err.println("This shouldn't happen!");
			return ; //World še ni postavljen!
		}
		PathNode mapPN[][] = new PathNode[w][h]; //Vse je null!
		PathNode rootNode = new PathNode(positionFrom);
		rootNode.obiskan = true;
		PriorityQueue<PathNode> lstCandidates = new PriorityQueue<PathNode>();
		
		PriorityQueue<PathNode> nodes= new PriorityQueue<PathNode>();
		nodes.add(rootNode);
		
		PathNode top;
		Pos2 selectedPos = null;
		//Dokler ne pridemo do prave pozicije ali se ne ustavimo, če se ustavimo na praznem stacku pomeni da
		//pozicija ni na voljo in vrnemo kar prezno pot
		//Stack<PathNode> pathToFollow = new Stack<PathNode>();
		while((top = nodes.poll()) != null)   
		{
			if(top.pos.equals(to))
				break;
			//Pogledamo vse 4 sosede
			for(int i = 0; i < World.MOVES.length; i++)
			{
				int nx = World.MOVES[i].x + top.pos.x;
				int ny = World.MOVES[i].y + top.pos.y;
				int ndist = top.dist+1;
				//kako dobiti pravo koordinato?
				int mapx = nx - agentWorld.getObservationArea().minX;
				int mapy = ny - agentWorld.getObservationArea().minY;
				//Ali smo izven dovoljenega območja
				if(nx < agentWorld.getObservationArea().minX || nx > agentWorld.getObservationArea().maxX || 
						ny < agentWorld.getObservationArea().minY || ny > agentWorld.getObservationArea().maxY )
				{
					continue;
				}
				//neobstoječa lokacija
				if(mapPN[mapx][mapy] == null)
				{
					PathNode n = new PathNode(new Pos2(nx, ny), ndist, top);
					n.obiskan = true;
					n.previous = top;
					//Preverimo ali je točka posebna
					//1. ali je robna!
					//2. ali je neraziskana!
					if(agentWorld.getTile(n.pos) == World.TILE_NOT_VISITED)
					{
						lstCandidates.add(n); //ne damo na sklad!
					}
					else if(agentWorld.isWalkable(n.pos))
					{
						mapPN[mapx][mapy] = n;
						nodes.add(n); //damo na sklad ker lahko še obstaja kakšna pot do sem!
						//ali je robna točka?
						if(nx == agentWorld.getObservationArea().minX || nx == agentWorld.getObservationArea().maxX || 
								ny == agentWorld.getObservationArea().minY || ny == agentWorld.getObservationArea().maxY )
						{
							lstCandidates.add(n);
						}
					}
				} 
				else //Ta prostor smo že obiskali! Obstaja alternativna pot do te točke.
				{
					//updejtamo ta node
					PathNode  n = mapPN[mapx][mapy];
					if(n.dist > ndist) //Obstaja krajša pot!
					{
						n.previous = top;
						n.dist = ndist;
						n.obiskan = true;
						nodes.add(n);
					}
				}
			}
		}
		
		if(top == null)
		{
			//Naredi prazno pot!
			System.out.println("Prazna pot!"); 
		}
		else //Pot obstaja
		{
			System.out.println("Planned path!(Reverse printed order)");
			while(top != null)
			{
				if(top.previous != null) //Zadnega ne dodamo, ker itak že smo na tej poziciji
				{
					pathToFollow.add(top);
					System.out.println("" + top);
				}
				top = top.previous;
				
			}
		}
	}
	
	/**
	 * 
	 */
	public boolean  moveToNexPoint()
	{
		if(pathToFollow.size() == 0)
			return false;
		else
		{
			currentNode =  pathToFollow.pop();
			return true;
		}
	}
	
	public PathNode getPointOnPath()
	{
		
		return currentNode;
	}
	
}
